﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using System.IO;

namespace LDAPaper
{
    public static class Common
    {
        public static T[] SubArray<T>(this T[] data, int index, int length)
        {
            T[] result = new T[length];
            Array.Copy(data, index, result, 0, length);
            return result;
        }

        public static double[] Subtract(double[] data1, double[] data2)
        {
            double[] result = new double[data1.Length];
            for (int i = 0; i < result.Length; i++)
            {
                result[i] = data1[i] - data2[i];
            }
            return result;
        }

        public static double[] Sum(double[] data1, double[] data2)
        {
            double[] result = new double[data1.Length];
            for (int i = 0; i < result.Length; i++)
            {
                result[i] = data1[i] + data2[i];
            }
            return result;
        }

        public static double[] Multiply(double[,] matrix, double[] vector)
        {
            int m = matrix.GetLength(0);
            int n = matrix.GetLength(1);

            int nn = vector.GetLength(0);

            if (n != nn)
                throw new Exception("dimensions do not agree");

            double[] result = new double[m];

            for (int i = 0; i < m; i++)
            {
                double sum = 0;
                for (int j = 0; j < n; j++)
                {
                    sum += matrix[i, j] * vector[j];
                }
                result[i] = sum;
            }
            return result;
        }

        public static double MultiplyAndSumAbsolutes(double[] vector1, double[] vector2)
        {
            int m = vector1.Length;
            int n = vector2.Length;

            if (m != n)
                throw new Exception("dimensions do not agree");

            double sum = 0;
            for (int i = 0; i < m; i++)
            {
                sum += Math.Abs(vector1[i] * vector2[i]);
            }
            return sum;
        }

        public static double SumAbsolutes(this double[] data)
        {
            double result = 0;
            for (int i = 0; i < data.Length; i++)
            {
                result += Math.Abs(data[i]);
            }
            return result;
        }

        public static int MinIndex(this double[] data)
        {
            double minValue = double.MaxValue;
            int minIndex = -1;

            for (int i = 0; i < data.Length; i++)
            {
                if (data[i] < minValue)
                {
                    minIndex = i;
                    minValue = data[i];
                }
            }

            return minIndex;
        }

        public static int AbsoluteMinIndex(this double[] data)
        {
            double minValue = double.MaxValue;
            int minIndex = -1;

            for (int i = 0; i < data.Length; i++)
            {
                if (Math.Abs(data[i]) < minValue)
                {
                    minIndex = i;
                    minValue = Math.Abs(data[i]);
                }
            }

            return minIndex;
        }

        public static double[,] MatrixSubSet(this double[,] data, int rowCount)
        {
            double[,] rVal = new double[rowCount, data.GetLength(0) + 1];

            for (int i = 0; i < rVal.GetLength(0); i++)
                for (int j = 0; j < rVal.GetLength(1); j++)
                    rVal[i, j] = data[i, j];

            return rVal;
        }

        public static double AbsoluteMinValue(this double[] data)
        {
            double minValue = double.MaxValue;
            int minIndex = -1;

            for (int i = 0; i < data.Length; i++)
            {
                if (Math.Abs(data[i]) < minValue)
                {
                    minIndex = i;
                    minValue = Math.Abs(data[i]);
                }
            }

            return minValue;
        }

        public static double ConventionalDTW(double[] s1, double[] s2)
        {
            int L1 = s1.Length / 8;
            int L2 = s2.Length / 8;

            double[,] D = new double[L1 + 1, L2 + 1];

            for (int i = 0; i < L1 + 1; i++)
                for (int j = 0; j < L2 + 1; j++)
                    D[i, j] = Double.PositiveInfinity;

            D[0, 0] = 0;

            for (int i = 1; i < L1 + 1; i++)
            {
                for (int j = 1; j < L2 + 1; j++)
                {
                    double[] s1CurrentFrame = s1.SubArray((i - 1) * 8, 8);
                    double[] s2CurrentFrame = s2.SubArray((j - 1) * 8, 8);

                    //double currentError = Subtract(s1CurrentFrame, s2CurrentFrame).SumAbsolutes();

                    double[] weights = { 0.333731209096917, 0.333731209096917, 0.138539972915026, 
                                          0.138539972915026, 0.376777997456161, 0.376777997456161, 0.150950820531896, 0.150950820531896 };

                    double currentError = MultiplyAndSumAbsolutes( Subtract(s1CurrentFrame, s2CurrentFrame), weights );
                    
                    double[] oldErrors = { D[i - 1, j - 1], D[i, j - 1], D[i - 1, j] };

                    double oldError = oldErrors.Min();

                    D[i, j] = oldError + currentError;
                }
            }

            return D[L1, L2];
        }

        public static double OurDTW(double[] s1, double[] s2, double[,] weights)
        {
            int L1 = s1.Length / 8;
            int L2 = s2.Length / 8;

            int numFeatures = weights.GetLength(0);

            double[, ,] D = new double[L1 + 1, L2 + 1, numFeatures + 1];

            for (int i = 0; i < D.GetLength(0); i++)
                for (int j = 0; j < D.GetLength(1); j++)
                    for (int k = 0; k < D.GetLength(2); k++)
                        D[i, j, k] = Double.PositiveInfinity;

            for (int i = 0; i < D.GetLength(2); i++)
                D[0, 0, i] = 0;

            for (int i = 1; i < L1 + 1; i++)
            {
                for (int j = 1; j < L2 + 1; j++)
                {
                    double[] s1CurrentFrame = s1.SubArray((i - 1) * 8, 8);
                    double[] s2CurrentFrame = s2.SubArray((j - 1) * 8, 8);
                    double[] sFrameDiff = Subtract(s1CurrentFrame, s2CurrentFrame);
                    double[] currentError = Multiply(weights, sFrameDiff);

                    double[,] C = new double[currentError.Length + 1, 3];//TA
                    double[] CAbsSums = new double[3];

                    for (int k = 0; k < currentError.Length; k++)
                    {
                        C[k, 0] = currentError[k] + D[i - 1, j - 1, k];
                        C[k, 1] = currentError[k] + D[i - 1, j, k];
                        C[k, 2] = currentError[k] + D[i, j - 1, k];

                        CAbsSums[0] += Math.Abs(C[k, 0]);
                        CAbsSums[1] += Math.Abs(C[k, 1]);
                        CAbsSums[2] += Math.Abs(C[k, 2]);
                    }

                    double sumAbsDiff = sFrameDiff.SumAbsolutes();

                    C[numFeatures, 0] = sumAbsDiff + D[i - 1, j - 1, numFeatures];
                    C[numFeatures, 1] = sumAbsDiff + D[i - 1, j, numFeatures];
                    C[numFeatures, 2] = sumAbsDiff + D[i, j - 1, numFeatures];

                    int minIdx = CAbsSums.MinIndex();

                    for (int k = 0; k < currentError.Length; k++)
                    {
                        D[i, j, k] = C[k, minIdx];
                    }
                    D[i, j, numFeatures] = C[numFeatures, minIdx];
                }
            }

            double rVal = 0;

            for (int i = 0; i < D.GetLength(2); i++)
            {
                rVal += Math.Abs(D[L1, L2, i]);
            }

            return rVal;
        }

        public static Dictionary<GestureType, double[,]> GetWeights(string filePath)
        {
            Dictionary<GestureType, double[,]> rVal = new Dictionary<GestureType, double[,]>();

            using (TextReader reader = File.OpenText(filePath))
            {
                string[] firstItems = reader.ReadLine().Split(' ');

                int item1 = Convert.ToInt32(firstItems[0]);
                int item2 = Convert.ToInt32(firstItems[1]);
                int item3 = Convert.ToInt32(firstItems[2]);

                string line = reader.ReadLine();
                while (line != null)
                {
                    double[,] newMatrix = new double[item2, item3];

                    string[] lineItems = line.Split(' ');

                    GestureType gType = (GestureType)Enum.Parse(typeof(GestureType), lineItems[0]);

                    int ii = 0, jj = 0;
                    for (int i = 1; i < lineItems.Length; i++)
                    {
                        newMatrix[ii, jj] = Convert.ToDouble(lineItems[i]);

                        jj++;
                        if (jj == item3)
                        {
                            ii++;
                            jj = 0;
                        }
                    }

                    newMatrix = newMatrix.MatrixSubSet(Parameters.UsedFeatureCount);

                    rVal.Add(gType, newMatrix);

                    line = reader.ReadLine();
                }

            }
            
            return rVal;
        }
    }
}
